﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Aphysoft.Vate
{
    public static class MatrixHelper
    {
        /// <summary>
        /// Create rotation to face matrix.
        /// </summary>
        /// <param name="position"></param>
        /// <param name="target"></param>
        /// <param name="absoluteUp"></param>
        /// <returns></returns>
        public static Matrix CreateRotationToFace(Vector3 position, Vector3 target, Vector3 absoluteUp)
        {
            Vector3 direction = Vector3.Subtract(position, target);
            Vector3 right = Vector3.Cross(absoluteUp, direction);
            Vector3.Normalize(ref right, out right);
            Vector3 backwards = Vector3.Cross(right, absoluteUp);
            Vector3.Normalize(ref backwards, out backwards);
            Vector3 up = Vector3.Cross(backwards, right);
            Matrix rot = new Matrix(right.X, right.Y, right.Z, 0, up.X, up.Y, up.Z, 0, backwards.X, backwards.Y, backwards.Z, 0, 0, 0, 0, 1);

            return rot;
        }
    }
}
